#include "System.h"

Encoder OriginalEncoder;


/**************************************************************************
函数功能：读取编码器计数
入口参数：定时器
返回  值：编码器数值(代表速度)
**************************************************************************/
int Read_Encoder(uint8_t TIMX)
{
 int Encoder_TIM;    
 switch(TIMX)
 {
	case 2:  Encoder_TIM= (int)(short)TIM2 -> CNT;   TIM2 -> CNT=CNT_INIT;  break;
	case 3:  Encoder_TIM= (int)(short)TIM3 -> CNT;   TIM3 -> CNT=CNT_INIT;  break;
	case 4:  Encoder_TIM= (int)(short)TIM4 -> CNT;   TIM4 -> CNT=CNT_INIT;  break;
	case 5:  Encoder_TIM= (int)(short)TIM5 -> CNT;   TIM5 -> CNT=CNT_INIT;  break;
	default: Encoder_TIM=0;
 }
	return Encoder_TIM;
}

/**************************************************************************
函数功能：读取编码器数值并计算车轮速度，单位m/s
入口参数：无
返回  值：无
**************************************************************************/
void Get_Velocity_Form_Encoder(void)
{   
  //获取编码器的原始数据
	//1ms 采集的脉冲值
	float Encoder_1_pr,Encoder_2_pr; 
	OriginalEncoder.A=Read_Encoder(2);	
	OriginalEncoder.B=Read_Encoder(4);	

  Encoder_1_pr=OriginalEncoder.A; 
	Encoder_2_pr=-OriginalEncoder.B; 
	MOTOR_1.speed= Encoder_1_pr; 
	MOTOR_2.speed= Encoder_2_pr; 

}
